/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.rultax.blackice.example;

import com.rultax.blackice.Physics;
import com.rultax.blackice.ProcessingObject;
import com.rultax.blackice.controller.TensorDampingController;
import java.util.ArrayList;
import java.util.List;
import org.jbox2d.collision.shapes.PolygonShape;
import org.jbox2d.common.Color3f;
import org.jbox2d.common.MathUtils;
import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;
import org.jbox2d.dynamics.BodyDef;
import org.jbox2d.dynamics.BodyType;
import org.jbox2d.dynamics.Fixture;
import org.jbox2d.dynamics.FixtureDef;
import org.jbox2d.dynamics.joints.PrismaticJointDef;
import org.jbox2d.dynamics.joints.RevoluteJoint;
import org.jbox2d.dynamics.joints.RevoluteJointDef;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import processing.core.PApplet;
import processing.core.PConstants;
import processing.core.PVector;
import sun.nio.cs.ext.MS950_HKSCS;

/**
 *
 * @author scottw
 */
public class Car extends ProcessingObject{

    private static final float MAX_STEER_ANGLE = MathUtils.PI / 5;
    private static final float STEAR_SPEED = 1.75f;
    private static final float SIDEWAYS_FRICTION_FORCE = 10;
    private static final float HORSEPOWER = 40;
    private static final PVector CAR_STARTING_POS = new PVector(100, 100);
    
    private static final float BODY_WIDTH = 10;
    private static final float BODY_HEIGHT = BODY_WIDTH * 2;
    private static final float WHEEL_WIDTH = BODY_WIDTH / 3;
    private static final float WHEEL_HEIGHT = BODY_HEIGHT / 3;
    
    private static final PVector leftFrontWheelPosition = new PVector(-((BODY_WIDTH / 2) + WHEEL_WIDTH - 1), BODY_HEIGHT / 2);
    private static final PVector rightFrontWheelPosition = new PVector(((BODY_WIDTH / 2) + WHEEL_WIDTH), BODY_HEIGHT / 2);
    private static final PVector leftRearWheelPosition = new PVector(-((BODY_WIDTH / 2) + WHEEL_WIDTH - 1), -BODY_HEIGHT / 2);
    private static final PVector rightRearWheelPosition = new PVector(((BODY_WIDTH / 2) + WHEEL_WIDTH), -BODY_HEIGHT / 2);
    
    
    private static final Logger logger = LoggerFactory.getLogger(Car.class);
    
    private float engineSpeed;
    private float steeringAngle;   
    
    private Body body;
    private List<Body> wheels;
    
    private Body lfWheel;
    private Body rfWheel;    
    private RevoluteJoint lfJoint;
    private RevoluteJoint rfJoint;
    
    private TensorDampingController tds;
    
    public Car(PApplet parent, Physics physics){
	super(parent, physics);
	engineSpeed = 0;
	steeringAngle = 0;
	makeBody(physics);
    }
    
    private void makeBody(Physics physics){
	wheels = new ArrayList<Body>();
	
	BodyDef bodyDef = new BodyDef();
	bodyDef.linearDamping = 1;
	bodyDef.angularDamping = 1;
	bodyDef.position = physics.coordScreenToWorld(CAR_STARTING_POS).clone();	
	bodyDef.type = BodyType.DYNAMIC;
	body = physics.createBody(bodyDef);
	logger.debug(bodyDef.toString());
	
	
	BodyDef lfWheelDef = new BodyDef();
	lfWheelDef.type = BodyType.DYNAMIC;
	PVector lfWheelVector = new PVector(CAR_STARTING_POS.x, CAR_STARTING_POS.y);
	lfWheelVector.add(leftFrontWheelPosition);
	lfWheelDef.position = physics.coordScreenToWorld(lfWheelVector);
	lfWheel = physics.createBody(lfWheelDef);
	
	BodyDef lrWheelDef = new BodyDef();
	PVector lrWheelVector = new PVector(CAR_STARTING_POS.x, CAR_STARTING_POS.y);
	lrWheelVector.add(leftRearWheelPosition);
	lrWheelDef.position = physics.coordScreenToWorld(lrWheelVector);
	lrWheelDef.type = BodyType.DYNAMIC;
	Body lrWheel = physics.createBody(lrWheelDef);
	
	BodyDef rfWheelDef = new BodyDef();
	PVector rfWheelVector = new PVector(CAR_STARTING_POS.x, CAR_STARTING_POS.y);
	rfWheelVector.add(rightFrontWheelPosition);
	rfWheelDef.position = physics.coordScreenToWorld(rfWheelVector);
	rfWheelDef.type = BodyType.DYNAMIC;
	rfWheel = physics.createBody(rfWheelDef);
	
	BodyDef rrWheelDef = new BodyDef();
	PVector rrWheelVector = new PVector(CAR_STARTING_POS.x, CAR_STARTING_POS.y);
	rrWheelVector.add(rightRearWheelPosition);
	rrWheelDef.position = physics.coordScreenToWorld(rrWheelVector);
	rrWheelDef.type = BodyType.DYNAMIC;
	Body rrWheel = physics.createBody(rrWheelDef);
	
	PolygonShape bodyShape = new PolygonShape();
	bodyShape.setAsBox(physics.scalarScreenToWorld(BODY_WIDTH / 2), physics.scalarScreenToWorld(BODY_HEIGHT / 2));
	FixtureDef bodyfd = new FixtureDef();
	bodyfd.shape = bodyShape;
	bodyfd.density = 1;
	body.createFixture(bodyfd);
	
	PolygonShape lfShape = new PolygonShape();
	lfShape.setAsBox(physics.scalarScreenToWorld(WHEEL_WIDTH / 2), physics.scalarScreenToWorld(WHEEL_HEIGHT / 2));
	FixtureDef lfDef = new FixtureDef();
	lfDef.shape = lfShape;
	lfDef.density = 10;
	lfWheel.createFixture(lfDef);
	wheels.add(lfWheel);
	
	PolygonShape rfShape = new PolygonShape();
	rfShape.setAsBox(physics.scalarScreenToWorld(WHEEL_WIDTH / 2), physics.scalarScreenToWorld(WHEEL_HEIGHT / 2));
	FixtureDef rfDef = new FixtureDef();
	rfDef.shape = rfShape;
	rfDef.density = 10;
	rfWheel.createFixture(rfDef);
	wheels.add(rfWheel);
	
	PolygonShape lrShape = new PolygonShape();
	lrShape.setAsBox(physics.scalarScreenToWorld(WHEEL_WIDTH / 2), physics.scalarScreenToWorld(WHEEL_HEIGHT / 2));
	FixtureDef lrDef = new FixtureDef();
	lrDef.shape = lrShape;
	lrDef.density = 1;
	lrWheel.createFixture(lrDef);
	wheels.add(lrWheel);
	
	PolygonShape rrShape = new PolygonShape();
	rrShape.setAsBox(physics.scalarScreenToWorld(WHEEL_WIDTH / 2), physics.scalarScreenToWorld(WHEEL_HEIGHT / 2));
	FixtureDef rrDef = new FixtureDef();
	rrDef.shape = rrShape;
	rrDef.density = 1;
	rrWheel.createFixture(rrDef);
	wheels.add(rrWheel);
	
	RevoluteJointDef lfJointDef = new RevoluteJointDef();
	lfJointDef.initialize(body, lfWheel, lfWheel.getWorldCenter());
	lfJointDef.enableMotor = true;
	lfJointDef.maxMotorTorque = 10000;
	
	RevoluteJointDef rfJointDef = new RevoluteJointDef();
	rfJointDef.initialize(body, rfWheel, rfWheel.getWorldCenter());
	rfJointDef.enableMotor = true;
	rfJointDef.maxMotorTorque = 10000;
	
	lfJoint = physics.createRevoluteJoint(lfJointDef);
	rfJoint = physics.createRevoluteJoint(rfJointDef);
	
	PrismaticJointDef lrJointDef = new PrismaticJointDef();
	lrJointDef.initialize(body, lrWheel, lrWheel.getWorldCenter(), new Vec2(1,0));
	lrJointDef.enableLimit = true;
	lrJointDef.lowerTranslation = lrJointDef.upperTranslation = 0;
	
	PrismaticJointDef rrJointDef = new PrismaticJointDef();
	rrJointDef.initialize(body, rrWheel, rrWheel.getWorldCenter(), new Vec2(1, 0));
	rrJointDef.enableLimit = true;
	rrJointDef.lowerTranslation = lrJointDef.upperTranslation = 0;
	
	physics.createJoint(lrJointDef);
	physics.createJoint(rrJointDef);
	
	tds = new TensorDampingController();
	tds.setAxisAligned(10, 100);
	tds.addBodies(wheels);
    }
    
    @Override
    public void display() {
	Physics physics = getPhysics();
	PApplet screen = getParent();
	
	
	PVector pos = physics.getBodyScreenCoord(body);
	float a = body.getAngle();
	paint(screen, pos.x, pos.y, BODY_WIDTH, BODY_HEIGHT, a);
	
	for (Body wheel : wheels) {
//	    Fixture wheelFixture = wheel.getFixtureList();
//	    PolygonShape wheelShape = (PolygonShape)wheelFixture.getShape();
//	    Vec2[] vertices = wheelShape.getVertices();
//	    drawSolidPolygon(vertices, vertices.length, Color3f.BLACK);
	    
	    PVector lfPos = physics.getBodyScreenCoord(wheel);
	    float lfa = wheel.getAngle();
	    paint(screen, lfPos.x, lfPos.y, WHEEL_WIDTH, WHEEL_HEIGHT, lfa);
	    
	}
    }
    
    private void paint(PApplet screen, float x, float y, float w, float h, float a){
	screen.rectMode(PConstants.CENTER);
	screen.pushMatrix();
	screen.translate(x, y);
	screen.rotate(-a);
	screen.fill(175);
	screen.stroke(0);
	screen.rect(0, 0, w, h);
	screen.popMatrix();
    }
    
    private void killOrthogonalVelocity(Body targetBody){
	Vec2 localPoint = new Vec2();
	Vec2 velocity = targetBody.getLinearVelocityFromLocalPoint(localPoint);
	
	Vec2 sidewaysAxis = targetBody.getTransform().R.col2.clone();
	sidewaysAxis.mulLocal(Vec2.dot(velocity, sidewaysAxis));
	targetBody.setLinearVelocity(sidewaysAxis);
    }
    
    public void accelerate(){
	body.setAwake(true);
	engineSpeed = -HORSEPOWER;
    }
    
    public void brake(){
	engineSpeed = HORSEPOWER;
    }
    
    public void left(){
	steeringAngle = MAX_STEER_ANGLE;
    }
    
    public void right(){
	steeringAngle = -MAX_STEER_ANGLE;
    }
    
    public void coast(){
	engineSpeed = 0;
    }
    
    public void straight(){
	steeringAngle = 0;
    }
    
    public void adjust(float dt){
	//tds.step(dt);
	
	for (Body wheelBody : wheels) {
	    killOrthogonalVelocity(wheelBody);
	}
	
	Vec2 lDirection = new Vec2(lfWheel.getTransform().R.col2.x, lfWheel.getTransform().R.col2.y);
	lDirection.mulLocal(engineSpeed);
	Vec2 rDirection = rfWheel.getTransform().R.col2.clone();
	rDirection.mulLocal(engineSpeed);
	lfWheel.applyForce(lDirection, lfWheel.getWorldCenter());
	rfWheel.applyForce(rDirection, rfWheel.getWorldCenter());
	
	float mspeed = steeringAngle - lfJoint.getJointAngle();
	
	if(steeringAngle == 0)
	    lfJoint.setMotorSpeed(mspeed * STEAR_SPEED * 4);	
	else
	    lfJoint.setMotorSpeed(mspeed * STEAR_SPEED);
	mspeed = steeringAngle - rfJoint.getJointAngle();
	
	if(steeringAngle == 0)
	    rfJoint.setMotorSpeed(mspeed * STEAR_SPEED * 4);
	else
	    rfJoint.setMotorSpeed(mspeed * STEAR_SPEED);
	
    }
    
    @Override
    public String toString(){
	String result = "Car:";
	result += "\n\tBody: " + body.getPosition().x + ", " + body.getPosition().y;
	for (Body wheel : wheels) {	    
	    result += "\n\tWheel: " + wheel.getPosition().x + ", " + wheel.getPosition().y;
	}
	return result;
    }
}
